#pragma once

#include "../core/i_alloc.h"
#include "../core/i_blas.h"

namespace idl
{

template<typename T>
class CorrespondenceNCC
{
public:
  CorrespondenceNCC();
  CorrespondenceNCC(int cx, int cy, int width, const T* const* im_data, int im_width, int im_height);
  CorrespondenceNCC(int cx, int cy, int width, const T* const* im_data);
  CorrespondenceNCC(const CorrespondenceNCC<T>& Corr);
  CorrespondenceNCC& operator=(const CorrespondenceNCC<T>& Corr);
 ~CorrespondenceNCC();
  bool init(int cx, int cy, int width, const T* const* im_data, int im_width, int im_height);
  const T *data() const {return _data[0];};
  int size() const { return _width*_width; };
  int width() const { return _width; };
  int centroid_x() const { return _cx; };
  int centroid_y() const { return _cy; };
  bool empty() const { return (!_initialized); };
  float sumi() const { return _sumi; };
  float reci() const { return _reci; };
protected:
  void cleanup();
protected:
  T **_data;
  int _cx, _cy; /*center x, center y*/
  int _width;
  float _sumi, _reci;
  bool _initialized;
};

template<typename T>
CorrespondenceNCC<T>::CorrespondenceNCC() : _data(NULL), _cx(-1), _cy(-1), _sumi(0.f), _reci(0.f)
{
  _initialized = false;
}

template<typename T>
CorrespondenceNCC<T>::CorrespondenceNCC(int cx, int cy, int width, const T* const* im_data, int im_width, int im_height) : _cx(cx), _cy(cy), _width(width), _sumi(0.f), _reci(0.f)
{
  int x, y, r, c, rad0, rad1, first_y, last_y, first_x, last_x, i, ww;
  float val, delta, sumii = 0.f;
  if (cx < 0 || cx > im_width - 1 || cy < 0 || cy > im_height - 1)
  {
    return;
  }
  _initialized = false;
  ww = _width*_width;
  if (im_data)
  {
    _data = i_alloc2<T>(_width, _width);
    rad0 = _width/2;
    rad1 = (_width - rad0 - 1);
    first_y = _cy - rad0;
    last_y  = _cy + rad1;
    first_x = _cx - rad0;
    last_x  = _cx + rad1;
    if (_data)
    {
      /*assign values:*/
      for (r = 0, y = first_y; y <= last_y; ++y, ++r)
      {
        if (y < 0 || y >= im_height)
        {
          i_zero(_data[r], _width);
        }
        else
        {
          for (c = 0, x = first_x; x <= last_x; ++x, ++c)
          {
            if (x < 0 || x >= im_width)
            {
              _data[r][c] = (T)0;
            }
            else
            {
              _data[r][c] = im_data[y][x];
            }
          }
        }
      }
      /*compute sumi, sumi2*/
      const T *p_data = _data[0];
      for (i = 0; i<ww; ++i)
      {
        val = (float)(p_data[i]);
        _sumi  += val;
        sumii += val*val;
      }
      delta = i_sqrt((float)(ww)*sumii - (_sumi*_sumi));
      _reci = i_rec(delta);
      _initialized = true;
    }
  }
}

/*ingore the image boundary conditions, faster*/
template<typename T>
CorrespondenceNCC<T>::CorrespondenceNCC(int cx, int cy, int width, const T* const* im_data) : _cx(cx), _cy(cy), _width(width), _sumi(0.f), _reci(0.f)
{
  int x, y, r, c, rad0, rad1, first_y, last_y, first_x, last_x, i, ww;
  float val, delta, sumii = 0.f;
  _initialized = false;
  ww = _width*_width;
  if (im_data)
  {
    _data = i_alloc2<T>(_width, _width);
    rad0 = _width / 2;
    rad1 = (_width - rad0 - 1);
      first_y = _cy - rad0;
    last_y = _cy + rad1;
    first_x = _cx - rad0;
    last_x = _cx + rad1;
    if (_data)
    {
      /*assign values:*/
      for (r = 0, y = first_y; y <= last_y; ++y, ++r)
      {
        for (c = 0, x = first_x; x <= last_x; ++x, ++c)
        {
            _data[r][c] = im_data[y][x];
        }
      }
      /*compute sumi, sumi2*/
      const T *p_data = _data[0];
      for (i = 0; i<ww; ++i)
      {
        val = (float)(p_data[i]);
        _sumi += val;
        sumii += val*val;
      }
      delta = i_sqrt((float)(ww)*sumii - (_sumi*_sumi));
      _reci = i_rec(delta);
      _initialized = true;
    }
  }
}

template<typename T>
CorrespondenceNCC<T>::CorrespondenceNCC(const CorrespondenceNCC<T>& Corr)
{
  _cx = Corr.centroid_x();
  _cy = Corr.centroid_y();
  _width = Corr.width();
  _sumi = Corr.sumi();
  _reci = Corr.reci();
  _initialized = !Corr.empty();
  if (_initialized && Corr.data())
  {
    _data = i_alloc2<T>(_width, _width);
    if (!_data)
    {
      _initialized = false;
    }
    else
    {
      i_copy(Corr.data(), _data[0], _width*_width);
    }
  }
}

template<typename T>
CorrespondenceNCC<T>& CorrespondenceNCC<T>::operator=(const CorrespondenceNCC<T>& Corr)
{
  if (this->_initialized)
  {
    if (this->_data[0] == Corr.data())
    {
      return(*this);
    }
    else
    {
      this->cleanup();
    }
  }
  this->_cx = Corr.centroid_x();
  this->_cy = Corr.centroid_y();
  this->_width = Corr.width();
  this->_sumi = Corr.sumi();
  this->_reci = Corr.reci();
  this->_initialized = !Corr.empty();
  if (this->_initialized && Corr.data())
  {
    
    this->_data = i_alloc2<T>(_width, _width);
    if (!this->_data)
    {
      this->_initialized = false;
    }
    else
    {
      i_copy(Corr.data(), this->_data[0], _width*_width);
    }
  }
  return(*this);
}


template<typename T>
CorrespondenceNCC<T>::~CorrespondenceNCC()
{
  i_free2<T>(_data);
}

template<typename T>
void CorrespondenceNCC<T>::cleanup()
{
  _initialized = false;
  i_free2<T>(_data);
  _sumi = _reci = 0.f;
  _cx = _cy = 0;
  _width = 0;
}

template<typename T>
bool CorrespondenceNCC<T>::init(int cx, int cy, int width, const T* const* im_data, int im_width, int im_height)
{
  int x, y, r, c, rad0, rad1, first_y, last_y, first_x, last_x, i, ww;
  float val, delta, sumii = 0.f;
  if (cx < 0 || cx > im_width - 1 || cy < 0 || cy > im_height - 1)
  {
    return false;
  }
  cleanup();
  _width = width;
  _cx = cx;
  _cy = cy;
  _width = width;
  ww = _width*_width;
  if (im_data)
  {
    _data = i_alloc2<T>(_width, _width);
    rad0 = _width / 2;
    rad1 = (_width - rad0 - 1);
      first_y = _cy - rad0;
    last_y = _cy + rad1;
    first_x = _cx - rad0;
    last_x = _cx + rad1;
    if (_data)
    {
      /*assign values:*/
      for (r = 0, y = first_y; y <= last_y; ++y, ++r)
      {
        if (y < 0 || y >= im_height)
        {
          i_zero(_data[r], _width);
        }
        else
        {
          for (c = 0, x = first_x; x <= last_x; ++x, ++c)
          {
            if (x < 0 || x >= im_width)
            {
              _data[r][c] = (T)0;
            }
            else
            {
              _data[r][c] = im_data[y][x];
            }
          }
        }
      }
      /*compute sumi, sumi2*/
      for (i = 0; i<ww; ++i)
      {
        val = (float)(_data[0][i]);
        _sumi += val;
        sumii += val*val;
      }
      delta = i_sqrt((float)(ww)*sumii - (_sumi*_sumi));
      _reci = i_rec(delta);
      _initialized = true;
    }
  } 
  return _initialized;
}

/*This routine returns a floating point ncc similarity score [-1,1] between two patches, with 1 being the most similar and -1 the least*/
template <typename T>
float i_match_ncc(const CorrespondenceNCC<T>& patch_x, const CorrespondenceNCC<T>& patch_y)
{
  if (patch_x.empty() || patch_y.empty() || (patch_x.width() != patch_y.width()))
  {
    return -1.f;
  }
  float dd = 0.f;
  const T* data_x = patch_x.data();
  const T* data_y = patch_y.data();
  int length = patch_x.size();
  for (int i = 0; i<length; ++i)
  {
    dd += (float)(data_x[i]) * (float)(data_y[i]);
  }
  float score = ((float)length*dd - patch_x.sumi()*patch_y.sumi()) * patch_x.reci() * patch_y.reci();
  return score;
}

} /* namespace idl */